/*
 * angle.h
 *
 *  Created on: 2020/7/18
 *      Author: hailiang
 */

#ifndef ANGLE_H
#define ANGLE_H

#include <Eigen/Geometry>
#include <cmath>

const double D2R = (M_PI / 180.0);
const double R2D = (180.0 / M_PI);

class Angle {

public:
    static double rad2deg(double rad) {
        return rad * R2D;
    }

    static double deg2rad(double deg) {
        return deg * D2R;
    }

    static float rad2deg(float rad) {
        return rad * static_cast<float>(R2D);
    }

    static float deg2rad(float deg) {
        return deg * static_cast<float>(D2R);
    }

    template <typename T, int Rows, int Cols>
    static Eigen::Matrix<T, Rows, Cols> rad2deg(const Eigen::Matrix<T, Rows, Cols> &array) {
        return array * R2D;
    }

    template <typename T, int Rows, int Cols>
    static Eigen::Matrix<T, Rows, Cols> deg2rad(const Eigen::Matrix<T, Rows, Cols> &array) {
        return array * D2R;
    }
};

#endif // ANGLE_H
